#pragma once

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include "slam_robot/serial_bridge.hpp" // 包含串口桥头文件以获取SpeedData结构

/**
 * @class OdometryPublisher
 * @brief 里程计发布器，使用速度数据计算机器人位置和朝向
 * 
 * 该类负责：
 * 1. 接收速度数据（来自SerialBridge）
 * 2. 计算机器人位置和朝向（积分）
 * 3. 发布里程计消息（/odom主题）
 * 4. 发布TF变换（odom→base_link）
 */
class OdometryPublisher {
public:
    /**
     * @brief 构造函数
     * @param parent_node 父节点指针
     * @param serial SerialBridge共享指针
     */
    OdometryPublisher(rclcpp::Node* parent_node, std::shared_ptr<SerialBridge> serial);
    
    /**
     * @brief 更新里程计数据并发布
     */
    void updateOdometry();

 private:
    std::mutex data_mutex_;
    double current_vx_ = 0;
    double current_vy_ = 0;
    double current_wz_ = 0;

private:
    void publishOdometry(const rclcpp::Time& now); // 发布里程计消息
    void publishTF(const rclcpp::Time& now);      // 发布TF变换
    
    rclcpp::Node* parent_node_;                    // 父节点指针
    std::shared_ptr<SerialBridge> serial_;       // SerialBridge实例
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_; // 里程计发布器
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // TF广播器
    rclcpp::TimerBase::SharedPtr timer_;          // 更新定时器
    
    // 位置状态
    double position_x_ = 0.0;   // 世界坐标系下X位置 (m)
    double position_y_ = 0.0;   // 世界坐标系下Y位置 (m)
    double yaw_ = 0.0;          // 朝向 (rad)
    
    rclcpp::Time last_time_;     // 上次更新时间
};